/**
 * @file mot_safe.h
 * @brief 运动安全检查算子接口
 * @version 0.1
 * @date 2019-11-06
 * @author xingxiaofan
 * @note 
 * @attention
*/
#pragma once
#include <vector>
#include <string>
namespace safety {
struct CartisianPos
{
    double x, y, z;
};
struct Quat
{
    double x, y, z, w;
};
struct Pose
{
    CartisianPos trans;
    Quat ori;
};
struct TargetPose
{
    Pose pose;
    uint8_t cfg[4];
    uint16_t ref_frame_id;
};

class JointSafetyMonitor
{
public:
    virtual ~JointSafetyMonitor();

    struct SafetyConfig
    {
        double pos_max;
        double pos_min;
        double vel_max;
        double acc_max;
        double torq_max;
        double pos_err;
        double torq_err;
    };
    virtual int reload_safety_config(SafetyConfig& config) = 0;
    virtual int check_pos(double pos, double vel) = 0;
    virtual int check_vel(double vel, double acc) = 0;
    virtual int check_torq(double cmd_torq) = 0;
    virtual double check_stop_distance(double vel, double max_dec, double* stop_time) = 0;

private:
    SafetyConfig cfg_;
};
class SafeZoneItf
{
public:
    virtual ~SafeZoneItf();
    virtual int check_pos(CartisianPos& pose, CartisianPos& vel, double* dis) = 0;
    enum SafeZoneType
    {
        PLANE = 1,
        CUBE,
        SPHERE,
        DIAMOND
    };
    virtual SafeZoneType get_type() = 0;
};
class SafePlane : public SafeZoneItf
{
};
class SafeCube : public SafeZoneItf
{
};
class SafeSphere : public SafeZoneItf
{
};
class SafeCylinder : public SafeZoneItf
{
};
class RobotSafetyMonitor
{
public:
    virtual ~RobotSafetyMonitor() {}
    struct SafetyConfig
    {
        double tcp_pos_err;
        double tcp_ori_err;

        double linear_vel_max;
        double ori_vel_max;

        double linear_acc_max;
        double ori_acc_max;
    };

    virtual int reload_safety_config(SafetyConfig& config) = 0;

    /**
     * @brief 检查笛卡尔位置是否符合安全区域
     * @param position 机器人笛卡尔位置
     * @param vel 机器人笛卡尔速度方向向量
     * @param zone 安全区域
     * @return 0: 符合安全区域，
     * @return 1: 不符合安全区域，但正在远离安全区域 
     * @return 3：不符合安全区域，且正在朝更加危险运动
    */
    virtual int check_pos(const CartisianPos& position, const CartisianPos& vel_dir, const SafeZoneItf& zone, double* distance = nullptr) = 0;
    virtual int check_pose_err(const Pose& cmd_pose, const Pose& cur_pose, double* ds = nullptr, double* dori = nullptr) = 0;

    struct OriSafeZone
    {
    };
    virtual int check_ori(const Quat& ori, const Quat& ori_vel, OriSafeZone& zone) = 0;

    virtual int check_vel(double linear_vel, double ori_vel) = 0;

    virtual double check_stop_distance(double linear_vel, double max_dec, double* stop_time) = 0;

    std::vector<JointSafetyMonitor> servo_monitor_;
};
}  // namespace safety
